# 读取z>领空的点云  （参数：领空高度）
# 算出平均值作为飞镖的坐标
# 带入抛物线物理模型，预设一个点
# 写一个抛物线模型  （写一个python函数，输入为每隔0.1s获取的20组三维坐标点，输出为预测的3s后的三维坐标点）
# 根据预测的飞镖坐标算出云台角度（套自瞄）（输入：目标点坐标，输出：云台角度，参数：子弹初速度）
# 当飞镖飞离领空时连续开火
import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
from sklearn.cluster import DBSCAN
from geometry_msgs.msg import Twist
import numpy as np
import struct
import math
class AirDefense(Node):

    def __init__(self):
        super().__init__('air_defense')
        self.subscription = self.create_subscription(PointCloud2, '/mid360', self.callback, 10)
        self.subscription  
        self.publisher = self.create_publisher(Twist, '/cmd_gimbal', 10)
        self.publishertest = self.create_publisher(PointCloud2, '/guiji', 10)
        # 领空的高度
        self.declare_parameter('min_hight', 3.3)

    def callback(self, msg):
        # 二维数组，n行3列的矩阵
        point_cloud_bytes = msg.data
        # 解析字节流中的点云数据
        xyz = self.parse_pointcloud2(point_cloud_bytes)
        # 待聚类的点集
        point_3D_list=[]
        #寻找高度高于领空的点
        for i, point in enumerate(xyz):
            if(point[2]>self.get_parameter('min_hight').value):
                point_3D=np.array([point[0],point[1],point[2]])
                point_3D_list.append(point_3D)
        if point_3D_list!=[]:
            point_3D_array = np.array(point_3D_list)
            goal_point = np.mean(point_3D_array, axis=0)
            self.convert_to_point_cloud(point_3D_list)
            # self.convert_to_point_cloud(point_3D_list_mean.tolist())
            # print('point_3D_list:',point_3D_list)
            print('goal_point:',goal_point)
            # 发布消息
            # self.publisher.publish(gimbal_angle)cmd_gimbal


    # 将点集转换为点云
    def convert_to_point_cloud(self,point_set):
        # 创建PointCloud2消息
        point_cloud_msg = PointCloud2()
        # 设置PointCloud2消息的字段
        point_cloud_msg.fields.append(PointField(
            name="x", offset=0, datatype=PointField.FLOAT32, count=1))
        point_cloud_msg.fields.append(PointField(
            name="y", offset=4, datatype=PointField.FLOAT32, count=1))
        point_cloud_msg.fields.append(PointField(
            name="z", offset=8, datatype=PointField.FLOAT32, count=1))
        # 将点集转换为NumPy数组
        point_cloud_array = np.array(point_set)
        # 将点集转换为PointCloud2消息的数据
        point_cloud_msg.data = point_cloud_array.astype(np.float32).tobytes()
        # 设置PointCloud2消息的其他属性
        point_cloud_msg.header.frame_id = "map"
        point_cloud_msg.height = 1
        point_cloud_msg.width = len(point_set)
        point_cloud_msg.is_bigendian = False
        point_cloud_msg.point_step = 12
        point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width
        self.publishertest.publish(point_cloud_msg)

    # 读取点云数据
    def parse_pointcloud2(self,data):
        # 定义点云数据格式
        point_format = '<fff'      # 一个点由 3 个 float 组成
        # 解析数据段
        point_data = data
        point_count = len(point_data) // struct.calcsize(point_format)
        points = []
        for i in range(point_count):
            offset = i * struct.calcsize(point_format)
            point_values = struct.unpack_from(point_format, point_data, offset)
            point_dict = [point_values[0], point_values[1],point_values[2]]
            points.append(point_dict)
        out=np.array(points)
        return out

            
def main(args=None):
    rclpy.init(args=args)
    lidar_reader = AirDefense()
    rclpy.spin(lidar_reader)
    lidar_reader.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()